#include "include/bRigidBody.h"

bRigidBody::bRigidBody(bBullet* bullet, CSTyp typ, float mass, int playerControlled, bVector3* size, bVector3* pos)
{
	btVector3 halfExtends	= btVector3(btScalar(size->getX()/2.0), btScalar(size->getY()/2.0), btScalar(size->getZ()/2.0));
	btVector3 position		= btVector3(btScalar(pos->getX()), btScalar(pos->getY()), btScalar(pos->getZ()));
	btVector3 localInertia;

	switch(typ)
	{
	case CS_BOX		: this->colShape = new btBoxShape(halfExtends); break;
	case CS_SPHERE	: this->colShape = new btSphereShape(btScalar(size->getX())); break;
	default			: this->colShape = new btBoxShape(halfExtends); break;
	}

	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(position);
	this->colShape->calculateLocalInertia(btScalar(mass), localInertia);
	this->motionState = new btDefaultMotionState(transform);

	btRigidBody::btRigidBodyConstructionInfo rbInfo(btScalar(mass), this->motionState, this->colShape, localInertia);
	this->rigidBody = new btRigidBody(rbInfo);

	std::cout << playerControlled << std::endl;
	if(playerControlled)
	{
		this->rigidBody->setCollisionFlags(this->rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
		this->rigidBody->setActivationState(DISABLE_DEACTIVATION);
	}

	bullet->getDynamicsWorld()->addRigidBody(this->rigidBody);
}

bRigidBody::~bRigidBody()
{
	delete this->colShape;
	delete this->motionState;
	delete this->rigidBody;
}

void bRigidBody::removeFromWorld(bBullet* bullet)
{
	bullet->getDynamicsWorld()->removeRigidBody(this->rigidBody);
}

void bRigidBody::getPosition(bVector3* vec)
{
	btTransform trans;
	this->rigidBody->getMotionState()->getWorldTransform(trans);
	vec->setX(trans.getOrigin().getX());
	vec->setY(trans.getOrigin().getY());
	vec->setZ(trans.getOrigin().getZ());
}

void bRigidBody::setPosition(bVector3* pos)
{
	/*
	btTransform transform = this->rigidBody->getWorldTransform();
	transform.setOrigin(btVector3(btScalar(pos->getX()), btScalar(pos->getY()), btScalar(pos->getZ())));
	this->rigidBody->setWorldTransform(transform);
	*/

	btTransform transform;
	this->rigidBody->getMotionState()->getWorldTransform(transform);
	transform.setOrigin(btVector3(pos->getX(), pos->getY(), pos->getZ()));
	this->rigidBody->getMotionState()->setWorldTransform(transform);
	this->rigidBody->activate(true);
}

void bRigidBody::translate(float x, float y, float z)
{
	this->rigidBody->translate(btVector3(btScalar(x), btScalar(y),btScalar(z)));
}

void bRigidBody::getRotation(bQuaternion* quat)
{
	btTransform transform;
	transform.setIdentity();
	this->motionState->getWorldTransform(transform);
	btQuaternion q = transform.getRotation();

	quat->setW(q.w());
	quat->setX(q.x());
	quat->setY(q.y());
	quat->setZ(q.z());
}

void bRigidBody::isKinematic()
{
	this->rigidBody->setCollisionFlags(this->rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
}

void bRigidBody::disableDeactivation()
{
	this->rigidBody->setActivationState(DISABLE_DEACTIVATION);
}

void bRigidBody::pitch(float x)
{
	btQuaternion q;
	btTransform t;
	t.setIdentity();
	this->rigidBody->getMotionState()->getWorldTransform(t);
	
	q = t.getRotation();
	q.setValue(x, q.y(), q.z());
	this->rigidBody->getMotionState()->setWorldTransform(btTransform(q));
}

void bRigidBody::yaw(float y)
{
	btQuaternion q;
	btTransform t;
	t.setIdentity();
	this->rigidBody->getMotionState()->getWorldTransform(t);
	
	q = t.getRotation();
	q.setValue(q.x(), y, q.z());
	this->rigidBody->getMotionState()->setWorldTransform(btTransform(q));
}

void bRigidBody::roll(float z)
{
	btQuaternion q;
	btTransform t;
	t.setIdentity();
	this->rigidBody->getMotionState()->getWorldTransform(t);
	
	q = t.getRotation();
	q.setValue(q.x(), q.y(), z);
	this->rigidBody->getMotionState()->setWorldTransform(btTransform(q));
}

// b3d style
bVector3* bRigidBody::bb_getPosition()
{
	bVector3* vec = new bVector3();
	btTransform trans;
	this->rigidBody->getMotionState()->getWorldTransform(trans);
	vec->setX(trans.getOrigin().getX());
	vec->setY(trans.getOrigin().getY());
	vec->setZ(trans.getOrigin().getZ());

	return vec;
}

bQuaternion* bRigidBody::bb_getRotation()
{
	bQuaternion* quat = new bQuaternion();

	btTransform transform;
	transform.setIdentity();
	this->motionState->getWorldTransform(transform);
	btQuaternion q = transform.getRotation();

	quat->setW(q.w());
	quat->setX(q.x());
	quat->setY(q.y());
	quat->setZ(q.z());

	return quat;
}